#include "canbus/message.h"
#include "canbus/consts.h"

/**
 * @namespace canbus
 * @brief canbus
 */
namespace canbus {

/**
 * @class CanMsg22
 * @brief Concrete can message that stores spreader control information
 */
class CanMsg22: public CanMsg {
public:
  CanMsg22():
  CanMsg(),
  front_hydraulic(kHydraulicDefault),
  back_hydraulic(kHydraulicDefault),
  hoist_height(kHoistHeightDefault)
  {
    id_ = 22;
    for(int i=0;i<8;i++) {
      *(data_+i) = 0;
    }
  }

  CanMsg22(uint8_t* value): 
  CanMsg(value),
  front_hydraulic(kHydraulicDefault),
  back_hydraulic(kHydraulicDefault),
  hoist_height(kHoistHeightDefault)
  {
    id_ = 22;
    for(int i=0;i<8;i++) {
      *(data_+i) = 0;
    }
  }

  virtual ~CanMsg22() = default;

  void UpUpdateData() {}

  template <typename T>
  void DownUpdate(const T& input_data) {

    front_hydraulic = mathClamp<double>(input_data.front_hydraulic, 0, kHydraulicMax);
    back_hydraulic = mathClamp<double>(input_data.back_hydraulic, 0, kHydraulicMax);
    hoist_height = mathClamp<double>(input_data.hoist_height, 0, kHoistHeightDefault);

    *data_     = getTwin(front_hydraulic, 0);
    *(data_+1) = getTwin(front_hydraulic, 1);
    *(data_+2) = getTwin(back_hydraulic, 0);
    *(data_+3) = getTwin(back_hydraulic, 1);
    *(data_+4) = getTwin(hoist_height, 0);
    *(data_+5) = getTwin(hoist_height, 1);
    *(data_+6) = input_data.spreader_cmd;
    *(data_+7) = input_data.op_cmd;
  }

  static const int32_t ID;
  static const std::string NAME;

  uint16_t front_hydraulic;
  uint16_t back_hydraulic;
  uint16_t hoist_height;

  friend class CanbusHandler;
};

// const int32_t CanMsg22::ID = 22;
// const std::string CanMsg22::NAME = "control_spreader";

}